/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_sensors.h"
#include "filters.h"
#include "fc_state.h"
#include "pwm3901_polling.h"
#include "anoPTv8_report.h"
extern float F1_wave[12];

#define OPTICAL_RESOLUTION (0.2131946f) /*1m高度下 1个像素对应的位移，单位cm*/
#define OULIER_LIMIT (100)              /*光流像素输出限幅*/
#define VEL_LIMIT (80.f)                /*光流速度限幅*/

struct optical_flow fc_optical;
static pt1Filter_t optFilter_lpf[2];
static pt1Filter_t gyroFilter_lpf[2];
static float gyro_lpf[2];
static float opt_lpf[2];

#if USE_MID_FILTER
#define OPTICAL_FLOW_SAMPLES 5
static float optFlowBuffer_x[OPTICAL_FLOW_SAMPLES];
static float optFlowBuffer_y[OPTICAL_FLOW_SAMPLES];
struct quickMedianFilter_t
{
    float *buffer;
    uint8_t size;
    uint8_t index;
} opt_mid_filter[2];
float qiuckMedianFilter5(struct quickMedianFilter_t *filter, float data)
{

    filter->buffer[filter->index] = data;
    filter->index++;
    if (filter->index >= filter->size)
    {
        filter->index = 0;
    }
    return quickMedianFilter5f(filter->buffer);
}
void quickMedianFilter5Init(struct quickMedianFilter_t *filter, uint8_t size, float *buffer)
{
    filter->buffer = buffer;
    filter->size = size;
    filter->index = 0;
}
#endif

void sensor_opticlFlow_init(void)
{
}

void opticalflow_sensor_handle_init(void)
{
    pt1FilterInit(&optFilter_lpf[X], pt1FilterGain(80, 0.01f));
    pt1FilterInit(&optFilter_lpf[Y], pt1FilterGain(80, 0.01f));
    pt1FilterInit(&gyroFilter_lpf[X], pt1FilterGain(10, 0.01f));
    pt1FilterInit(&gyroFilter_lpf[Y], pt1FilterGain(10, 0.01f));
}
void opticalflow_sensor_handle(void)
{
    // if (fc_optical.is_valid != 1 || fc_optical.quality < 100 || fc_optical.quality > 4000)
    // {
    //     fc_optical.raw_x = 0;
    //     fc_optical.raw_y = 0;
    //     fc_optical.gfix_x = 0;
    //     fc_optical.gfix_y = 0;
    //     fc_optical.quality = 0;
    //     fc_optical.shutter = 0;
    //     return;
    // }
    fc_optical.raw_x = pmw3901_motion.delta_y;
    fc_optical.raw_y = pmw3901_motion.delta_x;

    opt_lpf[X] = pt1FilterApply(&optFilter_lpf[X], fc_optical.raw_x);
    opt_lpf[Y] = pt1FilterApply(&optFilter_lpf[Y], fc_optical.raw_y);

    gyro_lpf[X] = pt1FilterApply(&gyroFilter_lpf[X], -zdrone_state.attitudeRate.pitch * 4.5f);
    gyro_lpf[Y] = pt1FilterApply(&gyroFilter_lpf[Y], zdrone_state.attitudeRate.roll * 4.5f);

    fc_optical.gfix_x = opt_lpf[X] - gyro_lpf[X];
    fc_optical.gfix_y = opt_lpf[Y] - gyro_lpf[Y];

    fc_optical.quality = pmw3901_motion.surface_quality;
    fc_optical.shutter = pmw3901_motion.shutter;

    // F1_wave[0] = fc_optical.raw_x;
    // F1_wave[1] = fc_optical.raw_y;
    // F1_wave[2] = opt_lpf[X];
    // F1_wave[3] = opt_lpf[Y];
    // F1_wave[4] = gyro_lpf[X];
    // F1_wave[5] = gyro_lpf[Y];
    // F1_wave[6] = fc_optical.gfix_x;
    // F1_wave[7] = fc_optical.gfix_y;
}
